// Copyright 2022 Chen Jun
// Licensed under the MIT License.

#ifndef ARMOR_DETECTOR__DETECTOR_NODE_HPP_
#define ARMOR_DETECTOR__DETECTOR_NODE_HPP_

// ROS
#include <image_transport/image_transport.hpp>
#include <image_transport/publisher.hpp>
#include <image_transport/subscriber_filter.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

//yaml
#include <yaml-cpp/yaml.h>

// STD
#include <memory>
#include <string>
#include <vector>

#include "armor_detector/armor.hpp"
#include "armor_detector/pnp_solver.hpp"
#include "detector2tracker/msg/armors.hpp"
#include "armor_detector/ArmorDetectorNetwork.h"

namespace rm_auto_aim
{

class ArmorDetectorNode : public rclcpp::Node
{
public:
  ArmorDetectorNode(const rclcpp::NodeOptions & options);

private:
  /**
   * @brief 接受图片并进行图片处理
   * @details 同一时间只有一个处理实例
   * @param img_msg 
   */
  void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr img_msg);

  /**
   * @brief 初始化Detector,并返回指针
   * 
   * @return std::unique_ptr<Detector> 
   */
  std::unique_ptr<ArmorDetectorNetwork> initDetector();


  // 消息变换
  void createMessage(detector2tracker::msg::Armor &armor_msg,const Armor & armor, const cv::Mat & rvec, const cv::Mat & tvec);
  

  // debug
  void createDebugPublisher();
  void destroyDebugPublisher();
  void createMarker(const Armor & armor,const detector2tracker::msg::Armor &armor_msg);
  void publishMarkers();

  // Armor Detector
  std::unique_ptr<ArmorDetectorNetwork> detector_;

  // Detected armors publisher
  detector2tracker::msg::Armors armors_msg_;
  rclcpp::Publisher<detector2tracker::msg::Armors>::SharedPtr armors_pub_;

  // deubg image publisher
  bool debug_;
  std::shared_ptr<rclcpp::ParameterEventHandler> debug_param_handler_;
  std::shared_ptr<rclcpp::ParameterCallbackHandle> debug_cb_handle_;
  image_transport::Publisher debug_img_pub_;


  // Visualization marker publisher
  visualization_msgs::msg::Marker armor_marker_;
  visualization_msgs::msg::Marker text_marker_;
  visualization_msgs::msg::MarkerArray marker_array_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;

  // Camera info part
  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;

  cv::Point2f cam_center_;
  std::shared_ptr<sensor_msgs::msg::CameraInfo> cam_info_;
  std::unique_ptr<PnPSolver> pnp_solver_;

  // Image subscrpition
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;

  //yaml node
  YAML::Node yaml;
};

}  // namespace rm_auto_aim

#endif  // ARMOR_DETECTOR__DETECTOR_NODE_HPP_
